#include <Eigen/Dense>
#include <deque>
#include <iostream>
#include <cstring>
#include <fstream>
#include <vector>
#include <sstream>
#include <unistd.h>

#include "math_utils.h"

using namespace std;

void read_result(double& init_bias, std::vector<double>& timestamps, std::vector<Eigen::Vector3d>& imu_data,  std::vector<Eigen::Vector3d>& odometry_data, std::vector<Eigen::Vector2d> &vel_data, std::vector<Eigen::Vector3d>& pose_data);

void read_simulate(double& init_bias, std::vector<double>& timestamps, std::vector<Eigen::Vector3d>& imu_data,  std::vector<Eigen::Vector3d>& odometry_data, std::vector<Eigen::Vector3d> &vel_data, std::vector<Eigen::Vector3d>& pose_data, std::vector<Eigen::Vector3d>& gps_data);

Eigen::Vector2d compute_velocity(const Eigen::Vector3d& last_pose, const Eigen::Vector3d& last_odom, const Eigen::Vector3d& curr_odom, const double &dt);

std::deque<double> convert_deque(const std::vector<double>& timestamps);